#include <ErraticModel.hpp>
#include <Trajectory.h>
#include <library.h>
#include <MapTransform.h>
#include <PathSrv.h>
#include "CostMarker.h"

class TrajectoryPlanner{
public:
	/**
	 * @brief constructor
	 * @param simulation_time No of seconds for each simulation, stepping at 10 Hz
	 * @param model of the erratic with all the constants for simulation.
	 * 
	 */
	TrajectoryPlanner(double simulation_time, int xv_samples, int tv_samples, double cx, double cy, double ct, double cvl, double cvt);
	TrajectoryPlanner();
	void init(double simulation_time, int xv_samples, int tv_samples, double cx, double cy, double ct, double cvl, double cvt, fnavfn::Path::Request &req);
	void init(double simulation_time, int xv_samples, int tv_samples, double cx, double cy, double ct, double cvl, double cvt);
	ControlCommand get_best_trajectory();
	
private:
	int sim_time;
	int xv_samples;
	int tv_samples;
	double alpha, beta, gamma, delta, dt;
    
    Erratic fe;
	Trajectory traj; 
	MapTransform mapper;
	PathSrv P;
	CostMarker cm;

    tf::TransformListener listener;
	tf::StampedTransform transform;
	ros::Publisher cost_marker_pub;
	
	void _for_all_linear_velocities(double vt,Trajectory &btraj, ControlCommand &bcnt, double &best_cost );
	bool _generateTrajectory(Trajectory &tr, ControlCommand &cmd );
	double distance(TFFF from,TFFF to);
	double interpolate_gdist_cost(double x, double y, double res);
	double interpolate_obs_cost(double x, double y, double res);
	double calculate_pdist(double x, double y,double &howCloseToGoal);

	inline double update_velocity( double current_velocity, double required_velocity, double maximum_acceleration, double time_step ){
		double slow = required_velocity < current_velocity ? -1 : +1;
		return slow > 0 ? 
			std::min( required_velocity, current_velocity + slow * maximum_acceleration * time_step ):	
			std::max( required_velocity, current_velocity + slow * maximum_acceleration * time_step );
	}
	
	inline double update_x(double current_x, double linear_velocity, double orientation, double time_step ){
		return current_x + std::cos( orientation ) * linear_velocity * time_step;
	}
	
	inline double update_y(double current_y, double linear_velocity, double orientation, double time_step ){
		return current_y + std::sin( orientation ) * linear_velocity * time_step;
	}
	
	inline double update_t(double orientation, double angular_velocity, double time_step ){
		return orientation + angular_velocity * time_step;
	}
};
